#include "Brain.h"

const char Brain::className[] = "Brain";

#define method(class, name) {#name, &class::name}

Lunar<Brain>::RegType Brain::methods[] = {
	method(Brain, addStep),
	method(Brain, setDialog),
};


Brain::Brain(lua_State* L) {
	m_pscript = L;
	m_evelocity = NORMAL;
}

Brain::~Brain() {
}

void Brain::init() {

	// Call Lua method
	lua_settop(m_pscript,0);
	Lunar<Brain>::push(m_pscript,this);
	Lunar<Brain>::call(m_pscript,"loadCourse",0,0);

	Lunar<Brain>::push(m_pscript,this);
	Lunar<Brain>::call(m_pscript,"loadDialog",0,0);
}

osg::Vec3d Brain::updatePosition(const osg::Vec3d& position) {
	osg::Vec3d p = position;
	if((p - m_vfinish).length() > 0.1){
		m_vdirection = (m_vfinish - m_vstart);
		m_vdirection.normalize();
		p += (m_vdirection*m_evelocity)/40.0;
	}
	else {
		size_t i;
		for(i=0; i< m_vstep.size(); i++) {
			if(m_vstep[i] == m_vfinish) break; 
		}
		m_vstart = m_vfinish;
		if(i < m_vstep.size() - 1) {
			m_vfinish = m_vstep[i+1];
		}
		else {
			m_vfinish = m_vstep[0];
		}
	}
	return p;
}

osg::Quat Brain::updateAttitude(const osg::Quat& attitude) {
	double angle= 0;
	
	if (m_vdirection[0]>=0)
		angle = -acos(m_vdirection[1]);
	else
		angle = acos(m_vdirection[1]);

	osg::Quat att = osg::Quat(angle, osg::Vec3d(0.0, 0.0, 1.0));
	return att;
}

int Brain::addStep(lua_State* L) {
	// Read step position
	float x,y,z;

	int nbArgs = lua_gettop(L);
	if (nbArgs == 3 && lua_isnumber(L,1) && lua_isnumber(L,2) && lua_isnumber(L,3)) {
		x = lua_tonumber(L,1);
		y = lua_tonumber(L,2);
		z = lua_tonumber(L,3);
	}
	else {
		std::cerr << "Brain new step : wrong parameters" << std::endl;
		return 0;
	}

	m_vstep.push_back(osg::Vec3d(x,y,z));
	if(m_vstep.size() == 1) {
		m_vstart = m_vstep[0];
	}
	else if(m_vstep.size() == 2) {
		m_vfinish = m_vstep[1];
	}

	return 0;
}

int Brain::setDialog(lua_State* L) {
	Dialog* d = Lunar<Dialog>::check(L,1);
	if (d == NULL) {
		std::cerr << "Set brain : Wrong parameters" << std::endl;
		return 0;
	}
	m_pdialog = d;

	return 0;
}

